/*
 * Barometer.cpp
 *
 *  Created on: Jan 14, 2014
 *      Author: ppks
 */

#include "common.h"
#include "IMU_Common.h"
#include "i2c.h"
#include "FlightController_Common.h"
#include "MathMethods.h"

/******************************************************************************/
/* Macro Definition                                                           */
/******************************************************************************/

/******************************************************************************/
/* Global Variables Declaration                                               */
/******************************************************************************/

/******************************************************************************/
/* Local Variables Declaration                                                */
/******************************************************************************/
static ms561101ba_s ms561101ba_ctx;
static int32_t baroPressure;
static int32_t baroTemperature;
static int32_t baroPressureSum;

/******************************************************************************/
/* Local Functions Declaration                                                */
/******************************************************************************/
#if (BAROMETER_TYPE == MS561101BA)
static void Barometer_ReadCalibrationData(void);
static void MS561101BA_UT_Start(void);
static void MS561101BA_UP_Start(void);
static void MS561101BA_UP_Read(void);
static void MS561101BA_UT_Read(void);
#endif /* #if (BAROMETER_TYPE == MS561101BA) */

static void Barometer_RecordPressureHistory(void);
static void Barometer_Calculate(void);

/******************************************************************************/
/* Function Code                                                              */
/******************************************************************************/
/*******************************************************************************
* Function Name: Barometer_Init()
********************************************************************************
* Summary:
*
*
* Parameters:
*  None
*
* Return:
*  None
*
*******************************************************************************/
void Barometer_Init(void)
{
    /* Init variables */
    baroPressure = 0;
    baroTemperature = 0;

#if (BAROMETER_TYPE == MS561101BA)
    /* Wait 10ms */
    delay(10);
    /* Reset the device */
    CommI2C_WriteReg(BARO_ADDRESS, MS561101BA_RESET_REG, 0);
    /* Wait 100ms */
    delay(100);

    /* Read calibration data from the device */
    Barometer_ReadCalibrationData();
    /* Wait 10ms */
    delay(10);

    /* Send Start condition for Barometer temperature reading */
    MS561101BA_UT_Start();

    /* Record the time for the next reading */
    ms561101ba_ctx.deadline = currentLoopTimeStamp + 10000;
#endif /* (BAROMETER_TYPE == MS561101BA) */
}


#if (BAROMETER_TYPE == MS561101BA)
/*******************************************************************************
* Function Name: Barometer_ReadCalibrationData()
********************************************************************************
* Summary:
*
*
* Parameters:
*  None
*
* Return:
*  None
*
*******************************************************************************/
static void Barometer_ReadCalibrationData(void)
{
    union {uint16_t val; uint8_t raw[2]; } data;
    uint8_t i;
    uint8_t tmpBuf[2];

    for (i = 0; i < 6; i++)
    {
        CommI2C_ReadRegToBuffer(BARO_ADDRESS, 0xA2 + 2*i, tmpBuf, 2);
        /* revert the order of MSB and LSB */
        data.raw[1] = tmpBuf[0];
        data.raw[0] = tmpBuf[1];

        /* store the register value into structure */
        ms561101ba_ctx.c[i+1] = data.val;
    }
}

/*******************************************************************************
* Function Name: Barometer_ReadCalibrationData()
********************************************************************************
* Summary:
*
*
* Parameters:
*  None
*
* Return:
*  None
*
*******************************************************************************/
static void MS561101BA_UT_Start(void)
{
    CommI2C_SendRepeatStart(BARO_ADDRESS << 1);             /* I2C write direction */
    CommI2C_Write(MS561101BA_TEMPERATURE_REG + OSR_VALUE);  /* Register selection */
    CommI2C_SendStop();
}

/*******************************************************************************
* Function Name: MS561101BA_UP_SendStartCondition()
********************************************************************************
* Summary:
*  read uncompensated pressure value: send command first
*
* Parameters:
*  None
*
* Return:
*  None
*
*******************************************************************************/
static void MS561101BA_UP_Start(void)
{
    CommI2C_SendRepeatStart(BARO_ADDRESS << 1);             /* I2C write direction */
    CommI2C_Write(MS561101BA_PRESSURE_REG + OSR_VALUE);     /* Register selection */
    CommI2C_SendStop();
}

/*******************************************************************************
* Function Name: MS561101BA_UP_Read()
********************************************************************************
* Summary:
*  read uncompensated pressure value and update global structure
*
* Parameters:
*  None
*
* Return:
*  None
*
*******************************************************************************/
static void MS561101BA_UP_Read(void)
{
    uint8_t tmpBuf[3];

    /* Read Pressure value from the device */
    CommI2C_ReadRegToBuffer(BARO_ADDRESS, 0, tmpBuf, 3);

    /* Store the data into the global structure union in reversed byte order */
    ms561101ba_ctx.up.raw[2] = tmpBuf[0];
    ms561101ba_ctx.up.raw[1] = tmpBuf[1];
    ms561101ba_ctx.up.raw[0] = tmpBuf[2];
}

/*******************************************************************************
* Function Name: MS561101BA_UT_Read()
********************************************************************************
* Summary:
*  read uncompensated temperature value: read result bytes
*
* Parameters:
*  None
*
* Return:
*  None
*
*******************************************************************************/
static void MS561101BA_UT_Read(void)
{
    uint8_t tmpBuf[3];

    /* Read Pressure value from the device */
    CommI2C_ReadRegToBuffer(BARO_ADDRESS, 0, tmpBuf, 3);

    /* Store the data into the global structure union in reversed byte order */
    ms561101ba_ctx.ut.raw[2] = tmpBuf[0];
    ms561101ba_ctx.ut.raw[1] = tmpBuf[1];
    ms561101ba_ctx.ut.raw[0] = tmpBuf[2];
}
#endif /* #if (BAROMETER_TYPE == MS561101BA) */

/*******************************************************************************
* Function Name: Barometer_Calculate()
********************************************************************************
* Summary:
*
*
* Parameters:
*  None
*
* Return:
*  None
*
*******************************************************************************/
static void Barometer_Calculate(void)
{
#if (BAROMETER_TYPE == MS561101BA)
    int32_t delt;

    float dT       = (int32_t)ms561101ba_ctx.ut.val - (int32_t)((uint32_t)ms561101ba_ctx.c[5] << 8);
    float off      = ((uint32_t)ms561101ba_ctx.c[2] <<16) + ((dT * ms561101ba_ctx.c[4]) / ((uint32_t)1<<7));
    float sens     = ((uint32_t)ms561101ba_ctx.c[1] <<15) + ((dT * ms561101ba_ctx.c[3]) / ((uint32_t)1<<8));
    baroTemperature  = (dT * ms561101ba_ctx.c[6]) / ((uint32_t)1<<23);

    /* If temperature lower than 20st.C */
    if (baroTemperature < 0)
    {
        delt = baroTemperature;
        delt  = 5*delt*delt;
        off  -= delt>>1;
        sens -= delt>>2;
    }

    /* Update values for temperature and pressure */
    baroTemperature  += 2000;
    baroPressure     = (( (ms561101ba_ctx.up.val * sens ) /((uint32_t)1<<21)) - off)/((uint32_t)1<<15);
#endif /* #if (BAROMETER_TYPE == MS561101BA) */
}

/*******************************************************************************
* Function Name: Barometer_RecordPressureHistory()
********************************************************************************
* Summary:
*  Stores the pressure in history array
*
* Parameters:
*  None
*
* Return:
*  None
*
*******************************************************************************/
static void Barometer_RecordPressureHistory(void)
{
    static int32_t baroHistTab[BARO_TAB_SIZE];
    static uint8_t baroHistIdx;

    uint8_t indexplus1 = (baroHistIdx + 1);

    if (indexplus1 == BARO_TAB_SIZE)
    {
        indexplus1 = 0;
    }

    /* Records the data for previous pressure values */
    baroHistTab[baroHistIdx] = baroPressure;

    /* Check the difference between last two measurements */
    baroPressureSum += baroHistTab[baroHistIdx];
    baroPressureSum -= baroHistTab[indexplus1];

    /* Increment last index */
    baroHistIdx = indexplus1;
}

/*******************************************************************************
* Function Name: MS561101BA_UT_Read()
********************************************************************************
* Summary:
*  The very first UT conversion is started in the Initialization cycle
*  This function is called periodically from the main loop and updates the
*  data for temperature or pressure if the computation (~8.5ms actual, but 10ms delay)
*  is finished and starts the next computation cycle for either T or P
*
* Parameters:
*  None
*
* Return:
*  uint8_t state: 0 - no new data is available yet
*                 1 - new data is available
*                 2 - no new data available, but computation time
*
*******************************************************************************/
uint8_t Barometer_UpdateData(void)
{
#if (BAROMETER_TYPE == MS561101BA)
    if (ms561101ba_ctx.state == BARO_STATE_NO_NEW_DATA_BUT_COMPUTATION)
    {
        /* a third state is introduced here to isolate calculate() and smooth timecycle spike */
        ms561101ba_ctx.state = BARO_STATE_NO_NEW_DATA; /* Reset the state to no data here */
        Barometer_Calculate();
        return BARO_STATE_NO_NEW_DATA_BUT_COMPUTATION;
    }

    if ((int16_t)(currentLoopTimeStamp - ms561101ba_ctx.deadline) < 0)
    {
        return BARO_STATE_NO_NEW_DATA;
    }

    ms561101ba_ctx.deadline = currentLoopTimeStamp + 10000;  /* UT and UP conversion take 8.5ms so we do next reading after 10ms */

    if (ms561101ba_ctx.state == 0)
    {
        MS561101BA_UT_Read();
        MS561101BA_UP_Start();

        /* Record the pressure data to the history array */
        Barometer_RecordPressureHistory();

        ms561101ba_ctx.state = BARO_STATE_NEW_DATA_AVAILABLE;
        return BARO_STATE_NEW_DATA_AVAILABLE;
    }
    else
    {
        MS561101BA_UP_Read();
        MS561101BA_UT_Start();
        ms561101ba_ctx.state = BARO_STATE_NO_NEW_DATA_BUT_COMPUTATION;
        return BARO_STATE_NO_NEW_DATA_BUT_COMPUTATION;
    }
#else
    /* Different types of barometers may be added here using #elseif */
    return 0;
#endif /* #if (BAROMETER_TYPE == MS561101BA) */
}

/*******************************************************************************
* Function Name: Barometer_GetPressureValue()
********************************************************************************
* Summary:
*  Returns computed pressure value
*
* Parameters:
*  None
*
* Return:
*  None
*
*******************************************************************************/
int32_t Barometer_GetPressureValue(void)
{
    return (baroPressure);
}

/*******************************************************************************
* Function Name: Barometer_GetTemperatureValue()
********************************************************************************
* Summary:
*  Returns computed temperature value
*
* Parameters:
*  None
*
* Return:
*  None
*
*******************************************************************************/
int16_t Barometer_GetTemperatureValue(void)
{
    return (baroTemperature);
}

/*******************************************************************************
* Function Name: Barometer_GetEstimatedAltitude()
********************************************************************************
* Summary:
*  Returns estimated altitude based on the pressure and temperature data
*
* Parameters:
*  None
*
* Return:
*  None
*
*******************************************************************************/
uint8_t Barometer_GetEstimatedAltitude(void)
{
    int32_t  BaroAlt;
    static float baroGroundTemperatureScale;
    static float logBaroGroundPressureSum;
    static float vel = 0.0f;
    static uint16_t previousT;
    uint16_t currentT = micros();
    uint16_t dTime;

    dTime = currentT - previousT;

    if (dTime < BARO_UPDATE_INTERVAL)
    {
        /* Not enough time elapsed for new altitude calculation */
        return 0;
    }

    previousT = currentT;

    if (calibration.BaroCycles > 0)
    {
        logBaroGroundPressureSum = log(baroPressureSum);
        baroGroundTemperatureScale = ((int32_t)baroTemperature + 27315) * (2 * 29.271267f); /* 2 *  is included here => no need for * 2  on BaroAlt in additional LPF */
        calibration.BaroCycles--;
    }

    /* baroGroundPressureSum is not supposed to be 0 here
     * see: https://code.google.com/p/ardupilot-mega/source/browse/libraries/AP_Baro/AP_Baro.cpp
     */
    BaroAlt = (logBaroGroundPressureSum - log(baroPressureSum)) * baroGroundTemperatureScale;

    /* additional LPF to reduce baro noise (faster by 30 µs) */
    baroData.estAltitude = (baroData.estAltitude * 6 + BaroAlt ) >> 3;

    #if (defined(VARIOMETER) && (VARIOMETER != 2)) || !defined(SUPPRESS_BARO_ALTHOLD)
        /* P */
        int16_t error16 = constrain(AltHold - alt.estAltitude, -300, 300);
        applyDeadband(error16, 10); //remove small P parametr to reduce noise near zero position
        BaroPID = constrain((FC_Config.PID[PIDALT].P8 * error16 >>7), -150, +150);

        /* I */
        errorAltitudeI += FC_Config.PID[PIDALT].I8 * error16 >>6;
        errorAltitudeI = constrain(errorAltitudeI,-30000,30000);
        BaroPID += errorAltitudeI>>9; //I in range +/-60

        applyDeadband(accZ, ACC_Z_DEADBAND);

        static int32_t lastBaroAlt;

        /* could only overflow with a difference of 320m, which is highly improbable here */
        int16_t baroVel = mul((alt.estAltitude - lastBaroAlt) , (1000000 / BARO_UPDATE_INTERVAL));

        lastBaroAlt = alt.estAltitude;

        baroVel = constrain(baroVel, -300, 300); // constrain baro velocity +/- 300cm/s
        applyDeadband(baroVel, 10); // to reduce noise near zero

        // Integrator - velocity, cm/sec
        vel += accZ * ACC_VEL_SCALE * dTime;

        // apply Complimentary Filter to keep the calculated velocity based on baro velocity (i.e. near real velocity).
        // By using CF it's possible to correct the drift of integrated accZ (velocity) without loosing the phase, i.e without delay
        vel = vel * 0.985f + baroVel * 0.015f;

        //D
        alt.estSpeed = vel;
        applyDeadband(alt.estSpeed, 5);
        BaroPID -= constrain(FC_Config.PID[PIDALT].D8 * alt.estSpeed >> 4, -150, 150);
    #endif

    /* Return 1 which means that new data for estiamted altitude is available */
    return 1;
}
